
(* 
**********************************************************

		 COSY_PAK package chap2.m		

**********************************************************
*)


If [ TrueQ[ $VersionNumber >= 2.0 ],
     Off[ General::spell ];
     Off[ General::spell1 ] ]
     
(*  B E G I N     P A C K A G E  *)

BeginPackage["COSYPAK`chap2`"];

SS2Transf::usage = 
"SS2Transf[A,B,C,s]";

Ode2SS::usage = 
"Ode2SS[lhscoeff, rhscoeff]: Convers linear  ODE to State Space Eqn. 
 The ODE is in the format: y(n) + a1 y(n-1) + ... + an-1 y(1) + an y = 
 b0 u(n) + b1 u(n-1) + ... bn-1 u(1) + bn u   
 where lhscoeff = [1, a1,...,an],  rhscoeff = [b0,b1,...,bn]. 
 The output are the Matrix A and Vector B in the state equation: 
 	dX/dt = A x + B u.";

Linearize::usage = 
"Linearize[f, {x1,...,xn}, {x10,...,xn0}]: gives the linearization 
 of vector function f[x1,...,xn] = {f1[x1,...,xn],...,fn[x1,...,xn]}
 at the point {x10,...,xn0}. The length of the function variables 
 {x1,...,xn} must be equal to the length of the operating point 
 {x10,...,xn0}.";

Begin["`Private`"];

(* Function for transferring State Space Eqn to
a transfer function *)
SS2Transf[A_,B_,C_,s_] :=
Block[{transf,II,na1,na2,nb,nc},
      na1 = Dimensions[A][[1]]; 
      na2 = Dimensions[A][[2]];
      nb = Dimensions[B][[1]];
      nc = Dimensions[C][[1]];
      If[na1 != na2 ||nb != na1 || nc != na2,
       Print["Dim[A]= ", na1,"x",na2];
       Print["Dim[B]= ", nb,"x","1"];
       Print["Dim[C]= ", "1","x",nc];
       Return["Warring!! Dimensions Dismatch !!"]
       ];
      
      II=IdentityMatrix[na1]; 	
      transf=Together[C.Inverse[s II -A].B];
      Print["The transfer function is:",transf];	
      Return[transf]
	];


(* Function for transferring ODE to State Space Eqn *)
Ode2SS[num_,den_] :=
Block[{i,j,A,B,nn=(Length[num]-1),n=(Length[den]-1),
			bata,num1,den1,b0,temp},
	A = Table[0,{i,n},{j,n}];
	B = Table[0,{i,n}];
	If[nn<n,num1=Join[Table[0,{i,1,n-nn}],num],num1=num];
	(* Devide all parameter by a0 to make the first coefficient
		to be zero *)
	num1 = num1/den[[1]];
	den1 = den/den[[1]];
	
	b0 = num1[[1]];
	
	(* Take off the first element of numerator and denominator *)
	num1 = Drop[num1,1];
	den1 = Drop[den1,1]; 
	(* The A Matrix *)
	Do[A[[i,i+1]]=1,{i,1,n-1}];	
	Do[A[[n,i]]=-den1[[n+1-i]],{i,1,n}];
	(* The B Vector *)
	B[[1]] = num1[[1]] - den1[[1]]*b0;
	Do[temp = Sum[den1[[j]]*B[[i-j]],{j,1,i-1}];
	   B[[i]] = Simplify[num1[[i]] - temp - den1[[i]]*b0],
	   {i,2,n}
	   ];
	(* Print out the result *)   
	Print["The A Matrix is:  "];Print[" "];Print["  "];
	Print["   ",TableForm[A]];
	Print["     "];Print["     "];
	Print["The B Vector is:  "];Print["  "];
	Print["   ",TableForm[B]];
	Return[{A,B}]
	];	

(* Linearize the nonlinear system *)	
Linearize[f_,xvars_,xpoint_,uvars_,upoint_] :=
  Block[{d1,i,j,nfunc,nx,nxvars,nu,nuvars,xrule,urule,
  			A,B,a,b},
     nfunc = Length[f]; 
     nxvars = Length[xvars];
     nx = Length[xpoint];
     nuvars = Length[uvars];
     nu = Length[upoint];
 	
     (* Check the lengths of vars and points *)
     If [nx != nxvars,
      Print[StringForm[
      "Linearize::Incompatible no. of xvars (``) and base xpoints (``).",
       nxvars,nx]];
       Return[{}]];
     
     If[nu != nuvars,
      Print[StringForm[
      "Linearize::Incompatible no. of uvars (``) and base upoints (``).",
       nuvars,nu]];
         Return[{}]];

             
     (* Substitution Rule *)
     xrule = Table[xvars[[j]] -> xpoint[[j]], {j,1,nx}];
     If[nu != 0,
        urule = Table[uvars[[j]] -> upoint[[j]], {j,1,nu}];
        rule = Join[xrule,urule],
        rule = xrule
        ];
             
     
     (* Produce the A Matrix *)
     A = Array[a,{nfunc,nx}];
     Do[a[i,j] = D[f[[i]],xvars[[j]]]/.rule , {i,1,nfunc},{j,1,nx}];
     
     (* Produce the B Matrix *)
     If[nu != 0,
        B = Array[b,{nfunc,nu}];
        Do[b[i,j] = D[f[[i]],uvars[[j]]]/.rule , {i,1,nfunc},{j,1,nu}]
        ];
    
    (* Print out the result *)   
	Print["The A Matrix is:  "];Print[" "];Print["  "];
	Print["   ",TableForm[A]];
	If[nu != 0,
		Print["     "];Print["     "];
		Print["The B Vector is:  "];Print["  "];
		Print["   ",TableForm[B]];
	   ];
	Return[{A,B}]
	];	

   	
End[];
EndPackage[];

(*  E N D     P A C K A G E  *)

If [ TrueQ[ $VersionNumber >= 2.0 ],
     On[ General::spell ];
     On[ General::spell1 ] ]
